﻿

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;

namespace Oprah
{
    public partial class DAD : Window
    {
        public DAD()
        {
            InitializeComponent();
        }

        KinectSensor sensor;
        bool closing = false;
        const int skeletonCount = 6;
        Skeleton[] allSkeletons = new Skeleton[skeletonCount];

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensorChooser2.KinectSensorChanged += new DependencyPropertyChangedEventHandler(kinectSensorChooser2_KinectSensorChanged);

        }

        void kinectSensorChooser2_KinectSensorChanged(object sender, DependencyPropertyChangedEventArgs e)
        {
            KinectSensor old = (KinectSensor)e.OldValue;

            StopKinect(old);

            sensor = (KinectSensor)e.NewValue;

            if (sensor == null)
            {
                return;
            }




            var parameters = new TransformSmoothParameters
            {
                Smoothing = 0.3f,
                Correction = 0.0f,
                Prediction = 0.0f,
                JitterRadius = 1.0f,
                MaxDeviationRadius = 0.5f
            };
            sensor.SkeletonStream.Enable(parameters);

            sensor.SkeletonStream.Enable();
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(sensor_AllFramesReady);
            
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);

           
            
            try
            {
                sensor.Start();
            }
            catch (System.IO.IOException)
            {
                kinectSensorChooser2.AppConflictOccurred();
            }
        }

        void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (closing)
            {
                return;
            }

            //Get a skeleton
            Skeleton first = GetFirstSkeleton(e);

            if (first == null)
            {
                return;
            }



            //set scaled position
            ScalePosition(headImage, first.Joints[JointType.Head]);
            ScalePosition(leftEllipse, first.Joints[JointType.HandLeft]);
            ScalePosition(rightEllipse, first.Joints[JointType.HandRight]);

           
            //if ()
            //ProcessGesture(first.Joints[JointType.Head], first.Joints[JointType.HandLeft], first.Joints[JointType.HandRight], image1);

            GetCameraPoint(first, e);
            Point rhp = GetJointPoint(first.Joints[JointType.HandRight]);
            Point rsp = GetJointPoint(first.Joints[JointType.ShoulderRight]);

            //if (rhp.X != 0)
            //{
 
            //}
        }

        void GetCameraPoint(Skeleton first, AllFramesReadyEventArgs e)
        {

            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {
                if (depth == null ||
                    kinectSensorChooser2.Kinect == null)
                {
                    return;
                }


                //Map a joint location to a point on the depth map
                //head
                DepthImagePoint headDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.Head].Position);
                //left hand
                DepthImagePoint leftDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandLeft].Position);
                //right hand
                DepthImagePoint rightDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandRight].Position);
                //right shoulder
                DepthImagePoint rshoulderDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.ShoulderRight].Position);


                SkeletonPoint rightSkelPoint = depth.MapToSkeletonPoint(rightDepthPoint.X, rightDepthPoint.Y);

                SkeletonPoint shoulderSkelPoint = depth.MapToSkeletonPoint(rshoulderDepthPoint.X, rshoulderDepthPoint.Y);


                //Map a depth point to a point on the color image
                //head
                ColorImagePoint headColorPoint =
                    depth.MapToColorImagePoint(headDepthPoint.X, headDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //left hand
                ColorImagePoint leftColorPoint =
                    depth.MapToColorImagePoint(leftDepthPoint.X, leftDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //right hand
                ColorImagePoint rightColorPoint =
                    depth.MapToColorImagePoint(rightDepthPoint.X, rightDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);

                ProcessGesture(rightSkelPoint, shoulderSkelPoint);


                //if (rightDepthPoint.Depth - rshoulderDepthPoint.Depth > 50)
                //{
                //    Canvas.SetLeft(image1, rightDepthPoint.X);
                //    Canvas.SetTop(image1, rightDepthPoint.Y);
                //}
                //else 
                //{
                //    Canvas.SetLeft(image1, leftDepthPoint.X);
                //    Canvas.SetTop(image1, leftDepthPoint.Y);
                //}
                  
                
                //Set location
                //CameraPosition(headImage, headColorPoint);
                //CameraPosition(leftEllipse, leftColorPoint);
                //CameraPosition(rightEllipse, rightColorPoint);
            }
        }

        private Point GetJointPoint(Joint joint)
        {
            int depthX = 0;
            int depthY = 0;

            DepthImagePoint point = this.sensor.MapSkeletonPointToDepth(joint.Position, DepthImageFormat.Resolution640x480Fps30);

            depthX *= (int) this.canvas1.ActualWidth / sensor.DepthStream.FrameWidth;

            depthY *= (int)this.canvas1.ActualHeight / sensor.DepthStream.FrameWidth;

            return new Point((double)depthX, (double)depthY);

        }

        private void ProcessGesture(Joint head, Joint handleft, Joint handright, Joint shoulderrignt, Image pix)
        {
            SkeletonPoint vector = new SkeletonPoint();
            vector.X = ScaleVector(1280, handright.Position.X);
            vector.Y = ScaleVector(720, -handright.Position.Y);
            vector.Z = handright.Position.Z;

            handright.Position = vector;

            if (handleft.Position.Y > head.Position.Y)
            {
                Canvas.SetLeft(pix, handright.Position.X);
                Canvas.SetTop(pix, handright.Position.Y);
            }


        }

        //private void ProcessGesture(DepthImagePoint dip1, DepthImagePoint dip2)
        //{
        //    SkeletonPoint vector = new SkeletonPoint();
        //    vector.X = ScaleVector(1280, dip1.X);
        //    vector.Y = ScaleVector(720, -dip2.Y);
        //    vector.Z = dip1.Depth;

        //    dip1. = vector;

        //    if (handleft.Position.Y > head.Position.Y)
        //    {
        //        Canvas.SetLeft(pix, handright.Position.X);
        //        Canvas.SetTop(pix, handright.Position.Y);
        //    }


        //}

        private void ProcessGesture(SkeletonPoint dip1, SkeletonPoint dip2)
        {
            SkeletonPoint vector = new SkeletonPoint();
            vector.X = ScaleVector(1280, dip1.X);
            vector.Y = ScaleVector(720, -dip2.Y);
            vector.Z = dip1.Z;

            dip1 = vector;

            if (vector.Z < dip2.Z)
            {
                Canvas.SetLeft(image1, vector.X);
                Canvas.SetTop(image1, vector.Y);
            }


        }

        private float ScaleVector(int length, float position)
        {
            float value = (((((float)length) / 1f) / 2f) * position) + (length / 2);
            if (value > length)
            {
                return (float)length;
            }
            if (value < 0f)
            {
                return 0f;
            }
            return value;
        }


        Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    return null;
                }


                skeletonFrameData.CopySkeletonDataTo(allSkeletons);

                //get the first tracked skeleton
                Skeleton first = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();

                return first;

            }
        }

        private void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    //stop sensor 
                    sensor.Stop();

                    //stop audio if not null
                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }


                }
            }
        }

        private void CameraPosition(FrameworkElement element, ColorImagePoint point)
        {
            //Divide by 2 for width and height so point is right in the middle 
            // instead of in top/left corner
            Canvas.SetLeft(element, point.X - element.Width / 2);
            Canvas.SetTop(element, point.Y - element.Height / 2);

        }

        private void ScalePosition(FrameworkElement element, Joint joint)
        {
            //convert the value to X/Y
            //Joint scaledJoint = joint.ScaleTo(1280, 720); 

            //convert & scale (.3 = means 1/3 of joint distance)
            Joint scaledJoint = joint.ScaleTo(1280, 720, .0f, .0f);

            Canvas.SetLeft(element, scaledJoint.Position.X);
            Canvas.SetTop(element, scaledJoint.Position.Y);

        }


        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            closing = true;
            StopKinect(kinectSensorChooser2.Kinect);
        }



    }
}
